/* 
 * University of Plymouth Bioloid Robot Controller
 * for Atmel Mega 128
 * FIRA England Robot Football Team
 * - Based on Dynamixel Evaluation Example (Original Author : Byoung Soo Kim)
 * - Innitial support for executing Motion Editor Pages
 * Author : Joerg Wolf, joerg.wolf -xaxtx- plymouth.ac.uk
 * 
 * Version 0.11
 */
 
#define ENABLE_BIT_DEFINITIONS 
#include <inttypes.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/signal.h>
#include "robot.h"
#include "bioloid.h"
#include "dynamixel-comms.h"

// --- Comms Global Variables ---
volatile byte gbpRxInterruptBuffer[256]; 
byte gbpParameter[128];
byte gbRxBufferReadPointer;
byte gbpRxBuffer[128]; 
byte gbpTxBuffer[128];
byte DebugServoID = '\xff'; 
volatile byte gbRxBufferWritePointer;

uint8_t CheckServos(void);

void TxD80(byte bTxdData);
extern void TxDString(unsigned char *bData);
//byte RxD81(void);
//void MiliSec(word wDelayTime);

byte TxPacket(byte bID, byte bInstruction, byte bParameterLength);
byte RxPacket(byte bRxLength);
void PrintBuffer(byte *bpPrintBuffer, byte bLength);

//UART0 Rx Interrupt - write data to buffer

SIGNAL (SIG_UART0_RECV)
{
  gbpRxInterruptBuffer[(gbRxBufferWritePointer++)] = RXD0_DATA;
}


/*
TxPacket() send data to RS485.
TxPacket() needs 3 parameter; ID of Dynamixel, Instruction byte, Length of parameters.
TxPacket() return length of Return packet from Dynamixel.
*/
byte TxPacket(byte bID, byte bInstruction, byte bParameterLength) 
{
    byte bCount,bCheckSum,bPacketLength;

    gbpTxBuffer[0] = 0xff;
    gbpTxBuffer[1] = 0xff;
    gbpTxBuffer[2] = bID;
    gbpTxBuffer[3] = bParameterLength+2; //Length(Paramter,Instruction,Checksum)
    gbpTxBuffer[4] = bInstruction;
    for(bCount = 0; bCount < bParameterLength; bCount++)
    {
        gbpTxBuffer[bCount+5] = gbpParameter[bCount];
    }
    bCheckSum = 0;
    bPacketLength = bParameterLength+4+2;
    for(bCount = 2; bCount < bPacketLength-1; bCount++) //except 0xff,checksum
    {
        bCheckSum += gbpTxBuffer[bCount];
    }
    gbpTxBuffer[bCount] = ~bCheckSum; //Writing Checksum with Bit Inversion

    RS485_TXD;
    for(bCount = 0; bCount < bPacketLength; bCount++)
    {
        sbi(UCSR0A,6);//SET_TXD0_FINISH;
        TxD80(gbpTxBuffer[bCount]);
    }
    while(!CHECK_TXD0_FINISH); //Wait until TXD Shift register empty
    RS485_RXD;
    return(bPacketLength);
}

/*
RxPacket() read data from buffer.
RxPacket() need a Parameter; Total length of Return Packet.
RxPacket() return Length of Return Packet.
*/

byte RxPacket(byte bRxPacketLength)
{
#define RX_TIMEOUT_COUNT2   3000L  
#define RX_TIMEOUT_COUNT1  (RX_TIMEOUT_COUNT2*10L)  
  unsigned long ulCounter;
  byte bCount, bLength, bChecksum;
  byte bTimeout;

  bTimeout = 0;
  for(bCount = 0; bCount < bRxPacketLength; bCount++)
  {
    ulCounter = 0;
    while(gbRxBufferReadPointer == gbRxBufferWritePointer)
    {
      if(ulCounter++ > RX_TIMEOUT_COUNT1)
      {
        bTimeout = 1;
        break;
      }
    }
    if(bTimeout) break;
    gbpRxBuffer[bCount] = gbpRxInterruptBuffer[gbRxBufferReadPointer++];
  }
  bLength = bCount;
  bChecksum = 0;
  
  
  //#ifndef	DISABLE_DYNAMIXEL_WARNINGS
  
  if (gbpRxBuffer[2]==DebugServoID)
  {
	byte hex [100];
	
	sprintf(hex, "[");
	
	int j = 1;
	
	for (int i = 0; i < bCount; i ++, j +=3)
		sprintf (&hex[j], "%02x ", gbpRxBuffer[i]);
		
	sprintf(&hex[j], "]\r\n");
	TxDString(hex);
  }
  //#endif
  
  
  if(gbpTxBuffer[2] != BROADCASTING_ID)
  {
    if(bTimeout && bRxPacketLength != 255) 
    {
	#ifndef	DISABLE_DYNAMIXEL_WARNINGS
		  TxDString("\r\n [Error:RxD Timeout]");
	#endif
		  CLEAR_BUFFER;
    }
    
    if(bLength > 3) //checking is available.
    {
		  if(gbpRxBuffer[0] != 0xff || gbpRxBuffer[1] != 0xff ) 
		  {
	#ifndef	DISABLE_DYNAMIXEL_WARNINGS
			TxDString("\r\n [Error:Wrong Header]");
	#endif
			CLEAR_BUFFER;
			return 0;
		  }
		  if(gbpRxBuffer[2] != gbpTxBuffer[2] )
		  {
	#ifndef	DISABLE_DYNAMIXEL_WARNINGS
			TxDString("\r\n [Error:TxID != RxID]");
	#endif
			CLEAR_BUFFER;
			return 0;
		  }  
		  if(gbpRxBuffer[3] != bLength-4) 
		  {
	#ifndef	DISABLE_DYNAMIXEL_WARNINGS
			TxDString("\r\n [Error:Wrong Length]");
	#endif
			CLEAR_BUFFER;
			return 0;
		  }  
		  for(bCount = 2; bCount < bLength; bCount++) bChecksum += gbpRxBuffer[bCount];
		  if(bChecksum != 0xff) 
		  {
	#ifndef	DISABLE_DYNAMIXEL_WARNINGS
			TxDString("\r\n [Error:Wrong CheckSum]");
	#endif
			CLEAR_BUFFER;
			return 0;
		  }
    }
  }
  return bLength;
}






//Dynamixel Communication Function Execution Step.
// Step 1. Parameter Setting (gbpParameter[]). In case of no parameter instruction(Ex. INST_PING), this step is not needed.
// Step 2. TxPacket(ID,INSTRUCTION,LengthOfParameter); --Total TxPacket Length is returned
// Step 3. RxPacket(ExpectedReturnPacketLength); -- Real RxPacket Length is returned
// Step 4 PrintBuffer(BufferStartPointer,LengthForPrinting);

uint8_t PingDevice(uint8_t bCount){
	 uint8_t retval = 1;
	 
	 TxPacket(bCount,INST_PING,0);
	 uint8_t bRxPacketLength = RxPacket(255);
	 if(bRxPacketLength == DEFAULT_RETURN_PACKET_SIZE) 
	 {
	
	 }else{
		char temp[34]; sprintf(temp,"E:Device %i not connected\n\r",bCount);TxDString(temp);
		retval=0;
	 }
	  return retval;
}



/*
TxD80() send data to USART 0.
*/
void TxD80(byte bTxdData)
{
  while(!TXD0_READY);
  TXD0_DATA = bTxdData;
}

/*
TXD81() send data to USART 1.
*/
void TxD81(byte bTxdData)
{
  while(!TXD1_READY);
  TXD1_DATA = bTxdData;
}


/*
TxD8Hex() print data seperatly.
ex> 0x1a -> '1' 'a'.
*/
/*
void TxD8Hex(byte bSentData)
{
  byte bTmp;

  bTmp =((byte)(bSentData>>4)&0x0f) + (byte)'0';
  if(bTmp > '9') bTmp += 7;
  TxD8(bTmp);
  bTmp =(byte)(bSentData & 0x0f) + (byte)'0';
  if(bTmp > '9') bTmp += 7;
  TxD8(bTmp);
}
*/

/*
TXD32Dex() change data to decimal number system
*/
/*
void TxD32Dec(long lLong)
{
  byte bCount, bPrinted;
  long lTmp,lDigit;
  bPrinted = 0;
  if(lLong < 0)
  {
    lLong = -lLong;
    TxD8('-');
  }
  lDigit = 1000000000L;
  for(bCount = 0; bCount < 9; bCount++)
  {
    lTmp = (byte)(lLong/lDigit);
    if(lTmp) 
    {
      TxD8(((byte)lTmp)+'0');
      bPrinted = 1;
    }
    else if(bPrinted) TxD8(((byte)lTmp)+'0');
    lLong -= ((long)lTmp)*lDigit;
    lDigit = lDigit/10;
  }
  lTmp = (byte)(lLong/lDigit);
  TxD8(((byte)lTmp)+'0');
}



/*
RxD81() read data from UART1.
RxD81() return Read data.
*/
byte RxD81(void)
{
  while(!RXD1_READY);
  return(RXD1_DATA);
}



